#include <buola/chart/cxychart.h>
#include <buola/chart/plots/clineplot.h>
#include <buola/chart/plots/cscatterplot.h>
#include <buola/app/capp.h>
#include <buola/app/ccmdline.h>
#include <buola/algorithm/comparison.h>
#include <buola/zz.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zchar.h>
#include <buola/zz/operators.h>
#include <buola/mat/cquat.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        std::vector<double> lType1YesXY,lType1YesZ,lType1NoXY,lType1NoZ;
        std::vector<double> lType2YesXY,lType2YesZ,lType2NoXY,lType2NoZ;
        std::vector<double> lType3YesP,lType3YesR,lType3NoP,lType3NoR;
        {
        io::CTextWriter lType1(io::file("type1.data")),lType2(io::file("type2.data")),lType3(io::file("type3.data"));
        
        for(int i=0;i<cmd_line().ArgCount();i++)
        {
            io::CTextReader lS(io::arg(i));
            
            std::string lLine;
            
            auto lRule=+zz::ZReal<double>();
            auto lSkipper=zz::chr("adefilnor: ()");
            
            while(get_line(lS,lLine))
            {
                if(starts_with(lLine,"error:"))
                {
                    std::vector<double> lNums;
                    
                    zz::phrase_parse(lLine.begin(),lLine.end(),lRule,lSkipper,lNums);
                    mat::CVec3d lPosError(lNums[0],lNums[1],lNums[2]);
                    mat::CQuat_d lRotError(lNums[3],lNums[4],lNums[5],lNums[6]);
                    
                    get_line(lS,lLine);
                    get_line(lS,lLine);
                    zz::phrase_parse(lLine.begin(),lLine.end(),lRule,lSkipper,lNums);

                    mat::CVec3d lPosDiff(lNums[0],lNums[1],lNums[2]);
                    mat::CQuat_d lRotDiff(lNums[3],lNums[4],lNums[5],lNums[6]);
                    
                    bool lOk=norm(lPosDiff)<0.02 && rad2deg(arg(lRotDiff))<2;

                    if(norm(lPosError)==0.0) //rotational error only
                    {
                        mat::C3DRotation lRot(lRotError);
                        lRot=lRot/1_deg;
                        double lXY=sqrt(sq(lRot.b)+sq(lRot.c));
                        double lZ=std::abs(lRot.a);
                        lType1 << lXY << "," << lZ << ",";
                        if(lOk)
                        {
                            lType1 << "1\n";
                            lType1YesXY.push_back(lXY);
                            lType1YesZ.push_back(lZ);
                        }
                        else
                        {
                            lType1 << "0\n";
                            lType1NoXY.push_back(lXY);
                            lType1NoZ.push_back(lZ);
                        }
                    }
                    else if(arg(lRotError)==0.0) //positional error only
                    {
                        double lXY=sqrt(sq(lPosError.x())+sq(lPosError.y()));
                        double lZ=std::abs(lPosError.z());
                        lType2 << lXY << "," << lZ << ",";
                        if(lOk)
                        {
                            lType2 << "1\n";
                            lType2YesXY.push_back(lXY);
                            lType2YesZ.push_back(lZ);
                        }
                        else
                        {
                            lType2 << "0\n";
                            lType2NoXY.push_back(lXY);
                            lType2NoZ.push_back(lZ);
                        }
                    }
                    else //both kinds of errors
                    {
                        double lP=norm(lPosError);
                        double lR=rad2deg(arg(lRotError));
                        lType3 << lP << "," << lR << ",";
                        if(lOk)
                        {
                            lType3 << "1\n";
                            lType3YesP.push_back(lP);
                            lType3YesR.push_back(lR);
                        }
                        else
                        {
                            lType3 << "0\n";
                            lType3NoP.push_back(lP);
                            lType3NoR.push_back(lR);
                        }
                    }
                }
            }
        }
        }
        CApp lApp;
        chart::PXYChart lType2Chart=new chart::CXYChart;

        chart::PScatterPlot lYesPlot=new chart::CScatterPlot;
        lYesPlot->SetSeries(new chart::CSeries(lType2YesXY,lType2YesZ));
        lYesPlot->SetName(L"passed");
        lYesPlot->SetColor(img::CColor(L"darkgreen"));
        lYesPlot->SetPointShape(chart::CPlot::CIRCLE);
        lYesPlot->SetPointDiameter(5);
        
        chart::PScatterPlot lNoPlot=new chart::CScatterPlot;
        lNoPlot->SetSeries(new chart::CSeries(lType2NoXY,lType2NoZ));
        lNoPlot->SetName(L"failed");
        lNoPlot->SetColor(img::CColor(L"darkred"));
        lNoPlot->SetPointShape(chart::CPlot::CROSS);
        lNoPlot->SetPointDiameter(5);
        
        lType2Chart->AddPlot(lYesPlot);
        lType2Chart->AddPlot(lNoPlot);
        lType2Chart->Map(CSize_d(800,600));
        lType2Chart->SetTitle(L"positional error only");
        
        chart::PXYChart lType3Chart=new chart::CXYChart;

        lYesPlot=new chart::CScatterPlot;
        lYesPlot->SetSeries(new chart::CSeries(lType3YesP,lType3YesR));
        lYesPlot->SetName(L"passed");
        lYesPlot->SetColor(img::CColor(L"darkgreen"));
        lYesPlot->SetPointShape(chart::CPlot::CIRCLE);
        lYesPlot->SetPointDiameter(5);
        
        lNoPlot=new chart::CScatterPlot;
        lNoPlot->SetSeries(new chart::CSeries(lType3NoP,lType3NoR));
        lNoPlot->SetName(L"failed");
        lNoPlot->SetColor(img::CColor(L"darkred"));
        lNoPlot->SetPointShape(chart::CPlot::CROSS);
        lNoPlot->SetPointDiameter(5);
        
        lType3Chart->AddPlot(lYesPlot);
        lType3Chart->AddPlot(lNoPlot);
        lType3Chart->Map(CSize_d(800,600));
        lType3Chart->SetTitle(L"positional and rotational error");
        
        chart::PXYChart lType1Chart=new chart::CXYChart;

        lYesPlot=new chart::CScatterPlot;
        lYesPlot->SetSeries(new chart::CSeries(lType1YesXY,lType1YesZ));
        lYesPlot->SetName(L"passed");
        lYesPlot->SetColor(img::CColor(L"darkgreen"));
        lYesPlot->SetPointShape(chart::CPlot::CIRCLE);
        lYesPlot->SetPointDiameter(5);
        
        lNoPlot=new chart::CScatterPlot;
        lNoPlot->SetSeries(new chart::CSeries(lType1NoXY,lType1NoZ));
        lNoPlot->SetName(L"failed");
        lNoPlot->SetColor(img::CColor(L"darkred"));
        lNoPlot->SetPointShape(chart::CPlot::CROSS);
        lNoPlot->SetPointDiameter(5);
        
        lType1Chart->AddPlot(lYesPlot);
        lType1Chart->AddPlot(lNoPlot);
        lType1Chart->Map(CSize_d(800,600));
        lType1Chart->SetTitle(L"rotational error only");
        
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << pE.what() << "\n";
    }

    return buola_finish();
}